package edu.gatech.cc.liam.geometry.linear;

import java.util.ArrayList;
import java.util.Arrays;
import java.util.Collection;

import edu.gatech.cc.liam.core.Globals;
import edu.gatech.cc.liam.geometry.linear.convexhull.PolytopeProjectionPipe;
import edu.gatech.cc.liam.geometry.linear.convexhull.Preferences;
import edu.gatech.cc.liam.geometry.linear.programming.InteriorPoint;

public class Polytope extends ArrayList<Halfspace> {

	final private static boolean DEBUG_PRINTS = false;
	private static final double TOLERANCE = Preferences.TOLERANCE;
	
	private static final long serialVersionUID = -4207778686283908271L;
	
	public int getDimensions() {
		if(this.isEmpty())
			return 0;
		else
			return this.get(0).normal.length;
	}
	
	public Polytope() {
		super();
	}
	
	private Polytope(Polytope p) {
		super(p);
	}
	
	// [A b] where: Ax + b <= 0
	public Polytope(double[][] ineqalities) {
		for(int i=0; i<ineqalities.length; i++) {
			NPoint normal = new NPoint(Arrays.copyOf(ineqalities[i], ineqalities[i].length-1));
			this.add(new Halfspace(normal, ineqalities[i][ineqalities[i].length-1]));
		}
	}
	public Polytope(Collection<NPoint> theNormals, double offset) {
		for(NPoint np : theNormals) {
			np.normalize();
			this.add(new Halfspace(np, offset));
		}
	}
	public Polytope(NPoint[] theNormals, double[] offsets) {
		for(int i=0; i<theNormals.length; i++) {
			theNormals[i].normalize();
			this.add(new Halfspace(theNormals[i], offsets[i]));
		}
	}
	public PointSet getExtremePoints() {
		return PolytopeProjectionPipe.manifoldHull(this);
	}
	
//	public PointSet getPointSet(double[] initialHyperplaneForProjection) {
//		return manifoldHullOfHalfspaces(initialHyperplaneForProjection);
//	}

	public static Polytope makeRandomPolytope(int numDimensions, int numHypers) {
		Polytope r = new Polytope();
		for(int i=0; i<numHypers; i++) {
			r.add(Halfspace.makeRandomHalfspace(numDimensions));
		}
		return r;
	}

	/**
	 * A polytope defined as a convex combination of points
	 * @param extremePoints
	 */
	public static Polytope makeConvexCombination(PointSet extremePoints) {
		Polytope rP = new Polytope();
		// one var for each point value and one var for each extreme point
		int dimensions = extremePoints.dimensions();
		int numVars = dimensions + extremePoints.size();
		for(int d=0; d<dimensions; d++) {
			double[] normal = new double[numVars];
			normal[d] = -1.0;
			int pIndex = 0;
			for(NPoint p : extremePoints) {
				normal[dimensions + pIndex] = p.values[d];
				pIndex++;
			}
			rP.addEqualityConstraint(new Halfspace(normal, 0.0));		
		}
		// point weights sum to 1
		NPoint sumNorm = new NPoint(numVars, 1.0);
		for(int d=0; d<dimensions; d++)
			sumNorm.values[d] = 0.0;
		rP.addEqualityConstraint(new Halfspace(sumNorm, -1.0));
		
		// each weight is greater than 0
		for(int pIndex=0; pIndex<extremePoints.size(); pIndex++) {
			double[] normal = new double[numVars];
			normal[dimensions + pIndex] = -1.0;
			rP.add(new Halfspace(normal, 0.0));
		}
		return rP;
	}

	/**
	 * Constructs a hypercube with each dimension i bounded between min[i] and max[i]
	 * @param min a vector of minimum bounds
	 * @param max a vector of maximum bounds
	 * @return
	 */
	public static Polytope makeHypercube(double[] min, double[] max) {
		int dimensions = min.length;
		Polytope newPolytope = new Polytope();
		for(int i=0; i<dimensions; i++) {
			NPoint p = new NPoint(dimensions, 0.0);
			p.values[i] = 1.0;
			newPolytope.add(new Halfspace(p, -max[i]));
			p.scale(-1.0);
			newPolytope.add(new Halfspace(p,  min[i]));
		}
		return newPolytope;
	}
	/**
	 * Makes a hypercube with each dimension bounded between minBound and maxBound
	 * @param dimension
	 * @param minBound
	 * @param maxBound
	 * @return
	 */
	public static Polytope makeBounds(int dimensions, double minBound, double maxBound) {
		double[] minB = new double[dimensions];
		double[] maxB = new double[dimensions];
		Arrays.fill(minB, minBound);
		Arrays.fill(maxB, maxBound);
		return makeHypercube(minB, maxB);
	}
	
	@Override
	public Polytope clone() {
		Polytope dup = new Polytope();
		for(Halfspace aH : this)
			dup.add(aH.clone());
		return dup;
	}
	
	public Polytope shallowClone() {
		return new Polytope(this);
	}
	
	public double[][] toMatrix() {
		double [][] r = new double[this.size()][];
		int i=0;
		for(Halfspace h : this) {
			r[i] = h.toArray();
			i++;
		}
		return r;
	}
	public 	Halfspace[] toArray() {
		final Halfspace[] hyperArray = new Halfspace[this.size()];
		return this.toArray(hyperArray);
	}
	
	public void scale(double scaleFactor) {
		for(Halfspace h : this) {
			h.offset *= scaleFactor;
		}
	}
	public void translate(NPoint vector) {
		for(Halfspace h : this) {
			assert (new NPoint(h.normal).dotProduct(h.normal)-1.0) < TOLERANCE; // normal must be normalized
			h.offset -= vector.dotProduct(h.normal);
		}
	}

	public void minkowskiSumApproximation(Polytope aHull) {
		assert hasSameNormalsInSameOrder(aHull);
		for(int i=0; i<aHull.size(); i++) {
			this.get(i).offset += aHull.get(i).offset;
		}
	}
	public void normalze() {
		for(Halfspace h : this) 
			h.normalize();
	}
	public boolean hasSameNormalsInSameOrder(Polytope aHull) {
		if(aHull.size() != this.size())
			return false;
		for(int i=0; i<aHull.size(); i++) {
			if(! new NPoint(aHull.get(i).normal).softEquals(new NPoint(this.get(i).normal), TOLERANCE))
				return false;
		}
		return true;
	}

	/**
	 * Compares two polytope to see if they have the same halfspaces
	 * @param aHull
	 * @return
	 */
	public boolean softEquals(Polytope aHull) {
		return softEquals(aHull, Globals.TOLERANCE);
	}
	/**
	 * Compares two polytope to see if they have the same halfspaces
	 * @param aHull
	 * @return
	 */
	public boolean softEquals(Polytope aHull, double tolerance) {
		if(aHull.size() != this.size())
			return false;
		for(Halfspace h1 : this) {
			Double otherOffset = aHull.getOffset(new NPoint(h1.normal));
			if(otherOffset == null)
				return false;
			if( Math.abs(h1.offset-otherOffset) > tolerance )
				return false;
		}
		return true;
	}
	
	/**
	 * @param normal
	 * @return the offset for halfspace with the given normal. Returns null if such a halfspace doesn't exist.
	 */
	public Double getOffset(NPoint normal) {
		for(Halfspace h : this) {
			if(normal.softEquals(new NPoint(h.normal)))
				return h.offset;
		}
		return null;
	}
	
	public ArrayList<NPoint> getNormals() {
		ArrayList<NPoint> rSet = new ArrayList<NPoint>();
		for(Halfspace h : this) {
			rSet.add(new NPoint(h.normal));
		}
		return rSet;
	}
	
	public void addEqualityConstraint(Halfspace hyperplane) {
		this.add(hyperplane.clone());
		NPoint norm = new NPoint(hyperplane.normal);
		norm.scale(-1.0);
		this.add(new Halfspace(norm, -hyperplane.offset));
	}
	
	
	public boolean isFeasible() {
		return InteriorPoint.findHalfSpaceInteriorPoint(this).dist >= -TOLERANCE;
	}
	
	public boolean isFeasible(NPoint pt) {
		return getDistance(pt) <= TOLERANCE;
	}
	
	// returns the maximum distance a point is from any of the polytope's halfspaces
	// a negative value means the point is feasible
	public double getDistance(NPoint pt) {
		double max = Double.MIN_VALUE;
		for(Halfspace h : this) {
			max = Math.max(max, h.pointDistanceFromHyperplane(pt));
		}
		return max;
	}
	
	
	/**
	 * Returns the set of constraints which must be in the basis.
	 */
	public Polytope findEqualityConstraints() {
		Polytope canidates = this.shallowClone();
		int numberLastRemoved;
		do{
			InteriorPoint ip = InteriorPoint.findHalfSpaceInteriorPointWithAugmentedObjective(this, canidates);
			if(DEBUG_PRINTS)System.out.println("Second dist interior =" + ip.dist + " @" + Arrays.toString(ip.values) );
			ArrayList<Halfspace> toBeRemoved = ip.findNonAdjecentHypers(canidates);
			canidates.removeAll(toBeRemoved);
			numberLastRemoved = toBeRemoved.size();
		} while(numberLastRemoved != 0);
		return canidates;
	}
	
	@Override
	public String toString() {
		double largestAbsOffset = Double.MIN_VALUE; 
		for(Halfspace h : this) {
			largestAbsOffset = Math.max(Math.abs(h.offset), largestAbsOffset);
		}
		int leadingDigits = 0;
		while(largestAbsOffset >= 1.0) {
			leadingDigits ++;
			largestAbsOffset *= 0.1;
		}
		StringBuffer rStr = new StringBuffer("");
		rStr.append("With extreme points: " + this.getExtremePoints() + "\n");
		for(Halfspace h : this) {
			rStr.append(h.toStringFixedLength(leadingDigits));
		}
		return rStr.toString();
	}
	
	public String toStringAxb() {
		StringBuffer A = new StringBuffer("A = [");
		StringBuffer b = new StringBuffer("b = [");
		for(Halfspace h : this) {
			for(double d : h.normal) {
				A.append(d + ",");
			}
			A.setCharAt(A.length()-1, ';');
			A.append(' ');
			b.append(h.offset + ",");
		}
		A.setCharAt(A.length()-1, ']');
		b.setCharAt(b.length()-1, ']');
		return A.toString() + "\n" + b.toString() + "\n";
	}

	public static void main(String[] args) {
		ApproximateEuclideanBall aBall = new ApproximateEuclideanBall(2, 0.01);
		Polytope p = new Polytope(aBall, -1.0);
		System.out.println(p.toStringAxb());
		System.out.println(p);
		System.out.println("Extreme Points:");
		System.out.println(p.getExtremePoints());
		System.out.println("Most Interior Point:\n"
				+ InteriorPoint.findHalfSpaceInteriorPoint(p));
	}
	
}
